/*
 * Matrix4x4.cpp
 *
 *  Created on: 2011/06/24
 *      Author: naoki
 */

#include "NTEngine/NTEngine.h"
#include "NTEngine/Math/Function.h"
#include "NTEngine/Math/Vector3.h"
#include "NTEngine/Math/Vector4.h"
#include "NTEngine/Math/Matrix3x4.h"
#include "NTEngine/Math/Matrix4x4.h"

namespace NTEngine {
namespace Math {

Matrix4x4::Matrix4x4( const Matrix4x4& a )
: m11( a.m11 ), m12( a.m12 ), m13( a.m13 ), m14( a.m14 )
, m21( a.m21 ), m22( a.m23 ), m23( a.m23 ), m24( a.m24 )
, m31( a.m31 ), m32( a.m32 ), m33( a.m33 ), m34( a.m34 )
, m41( a.m41 ), m42( a.m42 ), m43( a.m43 ), m44( a.m44 ) {
}

void Matrix4x4::operator*=( const Matrix3x4& a ) {
	float tx = m11;
	float ty = m12;
	float tz = m13;
	m11 = tx * a.m11 + ty * a.m21 + tz * a.m31;
	m12 = tx * a.m12 + ty * a.m22 + tz * a.m32;
	m13 = tx * a.m13 + ty * a.m23 + tz * a.m33;
	m14 += tx * a.m14 + ty * a.m24 + tz * a.m34;
	tx = m21;
	ty = m22;
	tz = m23;
	m21 = tx * a.m11 + ty * a.m21 + tz * a.m31;
	m22 = tx * a.m12 + ty * a.m22 + tz * a.m32;
	m23 = tx * a.m13 + ty * a.m23 + tz * a.m33;
	m24 += tx * a.m14 + ty * a.m24 + tz * a.m34;
	tx = m31;
	ty = m32;
	tz = m33;
	m31 = tx * a.m11 + ty * a.m21 + tz * a.m31;
	m32 = tx * a.m12 + ty * a.m22 + tz * a.m32;
	m33 = tx * a.m13 + ty * a.m23 + tz * a.m33;
	m34 += tx * a.m14 + ty * a.m24 + tz * a.m34;
	tx = m41;
	ty = m42;
	tz = m43;
	m41 = tx * a.m11 + ty * a.m21 + tz * a.m31;
	m42 = tx * a.m12 + ty * a.m22 + tz * a.m32;
	m43 = tx * a.m13 + ty * a.m23 + tz * a.m33;
	m44 += tx * a.m14 + ty * a.m24 + tz * a.m34;
}

void Matrix4x4::operator*=( const Matrix4x4& a ) {
	float tx = m11;
	float ty = m12;
	float tz = m13;
	m11 = tx * a.m11 + ty * a.m21 + tz * a.m31 + m14 * a.m41;
	m12 = tx * a.m12 + ty * a.m22 + tz * a.m32 + m14 * a.m42;
	m13 = tx * a.m13 + ty * a.m23 + tz * a.m33 + m14 * a.m43;
	m14 = tx * a.m14 + ty * a.m24 + tz * a.m34 + m14 * a.m44;
	tx = m21;
	ty = m22;
	tz = m23;
	m21 = tx * a.m11 + ty * a.m21 + tz * a.m31 + m24 * a.m41;
	m22 = tx * a.m12 + ty * a.m22 + tz * a.m32 + m24 * a.m42;
	m23 = tx * a.m13 + ty * a.m23 + tz * a.m33 + m24 * a.m43;
	m24 = tx * a.m14 + ty * a.m24 + tz * a.m34 + m24 * a.m44;
	tx = m31;
	ty = m32;
	tz = m33;
	m31 = tx * a.m11 + ty * a.m21 + tz * a.m31 + m34 * a.m41;
	m32 = tx * a.m12 + ty * a.m22 + tz * a.m32 + m34 * a.m42;
	m33 = tx * a.m13 + ty * a.m23 + tz * a.m33 + m34 * a.m43;
	m34 = tx * a.m14 + ty * a.m24 + tz * a.m34 + m34 * a.m44;
	tx = m41;
	ty = m42;
	tz = m43;
	m41 = tx * a.m11 + ty * a.m21 + tz * a.m31 + m44 * a.m41;
	m42 = tx * a.m12 + ty * a.m22 + tz * a.m32 + m44 * a.m42;
	m43 = tx * a.m13 + ty * a.m23 + tz * a.m33 + m44 * a.m43;
	m44 = tx * a.m14 + ty * a.m24 + tz * a.m34 + m44 * a.m44;
}

bool Matrix4x4::operator==( const Matrix4x4& a ) const {
	for ( int i = 3; i >= 0; --i ) {
		for ( int k = 3; k >= 0; --k ) {
			if ( operator()( i, k ) != a( i, k ) ) {
				return false;
			}
		}
	}
	return true;
}

bool Matrix4x4::operator!=( const Matrix4x4& a ) const {
	return !( operator==( a ) );
}

float& Matrix4x4::operator ()( int r, int c ) {
	ASSERT( ( r >= 0 ) && ( r < 4 ) );
	ASSERT( ( c >= 0 ) && ( c < 4 ) );
	return ( &m11 )[ 4 * r + c ];
}

float Matrix4x4::operator ()( int r, int c ) const {
	ASSERT( ( r >= 0 ) && ( r < 4 ) );
	ASSERT( ( c >= 0 ) && ( c < 4 ) );
	return ( &m11 )[ 4 * r + c ];
}

void Matrix4x4::setPerspectiveTransform(
	float fov,
	float w,
	float h,
	float nc,
	float fc,
	bool zBufferIsLinear )
{
	float s = 1.F / tan( fov * 0.5F );
//	float r = h / w;
	float a = ( zBufferIsLinear ) ?
		( 1.F / ( nc - fc ) ) : ( fc / ( nc - fc ) );
//	float b = nc * a;
	m11 = s * ( h / w );
	m22 = s;
	m33 = a;
	m34 = nc * a;
	m43 = -1.F;
	m12 = m13 = m14 =
	m21 = m23 = m24 =
	m31 = m32 =
	m41 = m42 = m44 = 0;
}

void Matrix4x4::setOrthogonalTransform( float xMin, float xMax, float yMin, float yMax, float zMin, float zMax ) {
	m11 = -2.F / ( xMin - xMax );
	m14 = 1.F - xMax * m11;
	m22 = -2.F / ( yMin - yMax );
	m24 = 1.F - yMax * m22;
	m33 = 1.F / ( zMin - zMax );
	m34 = -zMax * m33;
	m44 = 1.F;
	m12 = m13 = m21 = m23 = m32 = m41 = m42 = m43 = 0;
}

void Matrix4x4::multiplyViewTransform( const Vector3& p, const Vector3& t, const Vector3& u ) {
	// まずはビュー行列の各成分を計算
	Vector3 x, y, z;
	z.setSub( p, t );
	z.normalize();
	x.setCross( u, z );
	x.normalize();
	y.setCross( z, x );
	float t14 = -( x.x * p.x + x.y * p.y + x.z * p.z );
	float t24 = -( y.x * p.x + y.y * p.y + y.z * p.z );
	float t34 = -( z.x * p.x + z.y * p.y + z.z * p.z );
	// 行列計算
	float tx = m11;
	float ty = m12;
	float tz = m13;
	m11 = tx * x.x + ty * y.x + tz * z.x;
	m12 = tx * x.y + ty * y.y + tz * z.y;
	m13 = tx * x.z + ty * y.z + tz * z.z;
	m14 += tx * t14 + ty * t24 + tz * t34;
	tx = m21;
	ty = m22;
	tz = m23;
	m21 = tx * x.x + ty * y.x + tz * z.x;
	m22 = tx * x.y + ty * y.y + tz * z.y;
	m23 = tx * x.z + ty * y.z + tz * z.z;
	m24 += tx * t14 + ty * t24 + tz * t34;
	tx = m31;
	ty = m32;
	tz = m33;
	m31 = tx * x.x + ty * y.x + tz * z.x;
	m32 = tx * x.y + ty * y.y + tz * z.y;
	m33 = tx * x.z + ty * y.z + tz * z.z;
	m34 += tx * t14 + ty * t24 + tz * t34;
	tx = m41;
	ty = m42;
	tz = m43;
	m41 = tx * x.x + ty * y.x + tz * z.x;
	m42 = tx * x.y + ty * y.y + tz * z.y;
	m43 = tx * x.z + ty * y.z + tz * z.z;
	m44 += tx * t14 + ty * t24 + tz * t34;
}

void Matrix4x4::setMul( const Matrix4x4& a, const Matrix3x4& b ) {
	float tx = a.m11;
	float ty = a.m12;
	float tz = a.m13;
	m11 = tx * b.m11 + ty * b.m21 + tz * b.m31;
	m12 = tx * b.m12 + ty * b.m22 + tz * b.m32;
	m13 = tx * b.m13 + ty * b.m23 + tz * b.m33;
	m14 = tx * b.m14 + ty * b.m24 + tz * b.m34 + a.m14;
	tx = a.m21;
	ty = a.m22;
	tz = a.m23;
	m21 = tx * b.m11 + ty * b.m21 + tz * b.m31;
	m22 = tx * b.m12 + ty * b.m22 + tz * b.m32;
	m23 = tx * b.m13 + ty * b.m23 + tz * b.m33;
	m24 = tx * b.m14 + ty * b.m24 + tz * b.m34 + a.m24;
	tx = a.m31;
	ty = a.m32;
	tz = a.m33;
	m31 = tx * b.m11 + ty * b.m21 + tz * b.m31;
	m32 = tx * b.m12 + ty * b.m22 + tz * b.m32;
	m33 = tx * b.m13 + ty * b.m23 + tz * b.m33;
	m34 = tx * b.m14 + ty * b.m24 + tz * b.m34 + a.m34;
	tx = a.m41;
	ty = a.m42;
	tz = a.m43;
	m41 = tx * b.m11 + ty * b.m21 + tz * b.m31;
	m42 = tx * b.m12 + ty * b.m22 + tz * b.m32;
	m43 = tx * b.m13 + ty * b.m23 + tz * b.m33;
	m44 = tx * b.m14 + ty * b.m24 + tz * b.m34 + a.m44;
}

void Matrix4x4::identity() {
	m11 = m22 = m33 = m44 = 1.F;
	m12 = m13 = m14 =
	m21 = m23 = m24 =
	m31 = m32 = m34 =
	m41 = m42 = m43 = 0;
}

void Matrix4x4::multiply(const Vector3& a, Vector4* out) const {
	out->x = m11 * a.x + m12 * a.y + m13 * a.z + m14;
	out->y = m21 * a.x + m22 * a.y + m23 * a.z + m24;
	out->z = m31 * a.x + m32 * a.y + m33 * a.z + m34;
	out->w = m41 * a.x + m42 * a.y + m43 * a.z + m44;
}

float Matrix4x4::determinant() const {
// flaot det = (m22 * (m33 * m44 - m34 * m43) +
//              m23 * (m34 * m42 - m32 * m44) +
//              m24 * (m32 * m43 - m33 * m42)) * m11 +
//             (m21 * (m34 * m43 - m33 * m44) +
//              m23 * (m31 * m44 - m34 * m41) +
//              m24 * (m33 * m41 - m31 * m43)) * m12 +
//             (m21 * (m32 * m44 - m34 * m42) +
//              m22 * (m34 * m41 - m31 * m44) +
//              m24 * (m31 * m42 - m32 * m41)) * m13 +
//             (m21 * (m33 * m42 - m32 * m43) +
//              m22 * (m31 * m43 - m33 * m41) +
//              m23 * (m32 * m41 - m31 * m42)) * m14;

	float a = m33 * m44 - m34 * m43;
	float b = m34 * m42 - m32 * m44;
	float c = m32 * m43 - m33 * m42;
	float d = m31 * m44 - m34 * m41;
	float e = m33 * m41 - m31 * m43;
	float f = m31 * m42 - m32 * m41;

	return m11 * (m22 * a + m23 * b + m24 * c) +
		m12 * (m21 * (-a) + m23 * d + m24 * e) +
		m13 * (m21 * (-b) + m22 * (-d) + m24 * f) +
		m14 * (m21 * (-c) + m22 * (-e) + m23 * (-f));
}

} // namespace Math
} // namespace NTEngine
